#pragma once

#include "opencv2/opencv.hpp"
#include "slStruct.h"
#include "read.h"
#include "inputParam.h"
#include "trinsic.h"
#include "pattern.h"
#include "decode.h"
#include "reconstruct.h"
#include "rectify.h"
#include "io.h"
#include "timer.h"
#include <string>
#ifndef slLog
#define slLog Log::getInstance()
#endif
#ifndef slTimer
#define slTimer Timer::getInstance()
#endif

namespace SL
{
    void generateGrayBrownWoOffsetPatternImages(std::string folder, InputParamStruct &inputParam)
    {
        auto param = inputParam.grayBrownWoOffsetCalibrateParam;
        int rowN = param.grayCodeRowN;
        int colN = param.grayCodeColN;
        Images whiteBlackImages = pattern::generateWhiteBlackImages(param.projectorWidth, param.projectorHeight);
        Images verticalImages = pattern::generateGrayCodePatternImages(colN, param.projectorWidth, param.projectorHeight, false, true);
        Images verticalReverseImages = pattern::generateGrayCodePatternImages(colN, param.projectorWidth, param.projectorHeight, true, true);
        Images horizontalImages = pattern::generateGrayCodePatternImages(rowN, param.projectorWidth, param.projectorHeight, false, false);
        Images horizontalReverseImages = pattern::generateGrayCodePatternImages(rowN, param.projectorWidth, param.projectorHeight, true, false);
        Images orderedImages;
        orderedImages.push_back(whiteBlackImages[0]);
        orderedImages.push_back(whiteBlackImages[1]);
        for (int i = 0; i < colN; i++)
        {
            orderedImages.push_back(verticalImages[i]);
            orderedImages.push_back(verticalReverseImages[i]);
        }
        for (int i = 0; i < rowN; i++)
        {
            orderedImages.push_back(horizontalImages[i]);
            orderedImages.push_back(horizontalReverseImages[i]);
        }
        io::createFolder(folder);
        for (int i = 0; i < orderedImages.size(); i++)
        {
            std::string filename = folder + "\\" + std::to_string(i) + ".png";
            imwrite(filename, orderedImages[i]);
        }
        std::string patternFolder = folder + "\\pattern";
        io::createFolder(patternFolder);
        for (int i = 0; i < orderedImages.size(); i++)
        {
            std::string filename = patternFolder + "\\" + std::to_string(i) + ".png";
            Mat image;
            if (i < 2 + 2 * colN)
            {
                image = orderedImages[i].row(0);
            }
            else
            {
                image = orderedImages[i].col(0);
            }
            imwrite(filename, image);
        }
    }

    void generateGrayBrownWOffsetPatternImages(std::string folder, InputParamStruct &inputParam)
    {
        auto param = inputParam.grayBrownWOffsetCalibrateParam;
        int rowN = param.grayCodeRowN;
        int colN = param.grayCodeColN;
        Images whiteBlackImages = pattern::generateWhiteBlackImages(param.projectorWidth, param.projectorHeight);
        Images verticalImages = pattern::generateGrayCodePatternImagesWOffset(colN, param.projectorWidth, param.projectorHeight, false, true);
        Images verticalReverseImages = pattern::generateGrayCodePatternImagesWOffset(colN, param.projectorWidth, param.projectorHeight, true, true);
        Images horizontalImages = pattern::generateGrayCodePatternImagesWOffset(rowN, param.projectorWidth, param.projectorHeight, false, false);
        Images horizontalReverseImages = pattern::generateGrayCodePatternImagesWOffset(rowN, param.projectorWidth, param.projectorHeight, true, false);
        Images orderedImages;
        orderedImages.push_back(whiteBlackImages[0]);
        orderedImages.push_back(whiteBlackImages[1]);
        for (int i = 0; i < colN; i++)
        {
            orderedImages.push_back(verticalImages[i]);
            orderedImages.push_back(verticalReverseImages[i]);
        }
        for (int i = 0; i < rowN; i++)
        {
            orderedImages.push_back(horizontalImages[i]);
            orderedImages.push_back(horizontalReverseImages[i]);
        }
        io::createFolder(folder);
        for (int i = 0; i < orderedImages.size(); i++)
        {
            std::string filename = folder + "\\" + std::to_string(i) + ".png";
            imwrite(filename, orderedImages[i]);
        }

        std::string patternFolder = folder + "\\pattern";
        io::createFolder(patternFolder);
        for (int i = 0; i < orderedImages.size(); i++)
        {
            std::string filename = patternFolder + "\\" + std::to_string(i) + ".png";
            Mat image;
            if (i < 2 + 2 * colN)
            {
                image = orderedImages[i].row(0);
            }
            else
            {
                image = orderedImages[i].col(0);
            }
            imwrite(filename, image);
        }
    }

    void calibrateGrayBrownWoOffsetImages(std::string folder, InputParamStruct &inputParam, std::string savePath)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        auto param = inputParam.grayBrownWoOffsetCalibrateParam;

        std::vector<std::string> folders = getAllFoldersWithinFolder(folder);
        int n = (int)folders.size();

        cameraImagesWithParamList.resize(n);

        ImagesList imagesList;
#pragma omp parallel for
        for (int i = 0; i < n; i++)
        {
            slLog.log("read %d image", i);
            cameraImagesWithParamList[i] = readCameraImages(folders[i]);
        }

        std::vector<Mat> cornerExtractImages;
        for (int i = 0; i < n; i++)
        {
            cornerExtractImages.push_back(cameraImagesWithParamList[i][0]->image);
        }
        CornersList cameraCornersList = calibrate::extractChessboradCorners(cornerExtractImages, param.cornerCols, param.cornerRows, false, "tradition");

        Images decodeImages;
        decodeImages.resize(n);
        // Images cameraImages;
        // #pragma omp parallel for
        for (int i = 0; i < n; i++)
        {
            slLog.log("decode %d image", i);
            Images &images = content.cameraGrayImages(i);
            // for (int i = 0; i < images.size(); i++)
            // {
            //     std::string filename = "D:\\code\\amiraSl\\test\\gray\\" + std::to_string(i) + ".png";
            //     imwrite(filename, images[i]);
            // }
            Mat decodeImage = decode::decodeGrayCodeBrownWoOffset(images, param.imageSize, param.projectorSize);
            std::string path = "D:\\code\\amiraSl\\test\\decode\\" + std::to_string(i) + "_horizontal.png";
            Mat horizontal;
            extractChannel(decodeImage, horizontal, 1);
            imwrite(path, horizontal);

            path = "D:\\code\\amiraSl\\test\\decode\\" + std::to_string(i) + "_vertical.png";
            Mat vertical;
            extractChannel(decodeImage, vertical, 0);
            imwrite(path, vertical);

            decodeImages[i] = decodeImage;
            // cameraImages.push_back(images[0]);
        }

        calibrate::calibrate(cameraCornersList, decodeImages, param.imageSize, param.projectorSize, param.cornerCols, param.cornerRows, param.squareWidth, param.squareHeight, param.windowSize, savePath);
    }

    void calibrateGrayBrownWOffsetImages(std::string folder, InputParamStruct &inputParam, std::string savePath)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        auto param = inputParam.grayBrownWOffsetCalibrateParam;

        std::vector<std::string> folders = getAllFoldersWithinFolder(folder);
        int n = (int)folders.size();

        cameraImagesWithParamList.resize(n);

        ImagesList imagesList;
#pragma omp parallel for
        for (int i = 0; i < n; i++)
        {
            slLog.log("read %d image in %s", i, folders[i]);
            cameraImagesWithParamList[i] = readCameraImages(folders[i]);
        }

        std::vector<Mat> cornerExtractImages;
        for (int i = 0; i < n; i++)
        {
            cornerExtractImages.push_back(cameraImagesWithParamList[i][0]->image);
        }
        CornersList cameraCornersList = calibrate::extractChessboradCorners(cornerExtractImages, param.cornerCols, param.cornerRows, false, "tradition");

        Images decodeImages;
        decodeImages.resize(n);
        // Images cameraImages;
        // #pragma omp parallel for
        for (int i = 0; i < n; i++)
        {
            slLog.log("decode %d image", i);
            Images &images = content.cameraGrayImages(i);
            // for (int i = 0; i < images.size(); i++)
            // {
            //     std::string filename = "D:\\code\\amiraSl\\test\\gray\\" + std::to_string(i) + ".png";
            //     imwrite(filename, images[i]);
            // }
            Mat decodeImage = decode::decodeGrayCodeBrownWOffset(images, param.imageSize, param.projectorSize, param.grayCodeColN, param.grayCodeRowN);

            std::string path = "D:\\code\\amiraSl\\test\\decode\\" + std::to_string(i) + "_vertical.tiff";
            Mat vertical;
            extractChannel(decodeImage, vertical, 0);
            imwrite(path, vertical);
            path = "D:\\code\\amiraSl\\test\\decode\\" + std::to_string(i) + "_horizontal.tiff";
            Mat horizontal;
            extractChannel(decodeImage, horizontal, 1);
            imwrite(path, horizontal);

            // decodeImages.push_back(decodeImage);
            decodeImages[i] = decodeImage;
            // cameraImages.push_back(images[0]);
        }

        calibrate::calibrate(cameraCornersList, decodeImages, param.imageSize, param.projectorSize, param.cornerCols, param.cornerRows, param.squareWidth, param.squareHeight, param.windowSize, savePath);
    }

    void reconstructeGrayBrownWoOffsetImages(std::string imageFolder, std::string trinsicFolder, std::string savePath, InputParamStruct &inputParam)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.grayBrownWoOffsetCalibrateParam;
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        content.cameraImagesWithParam = readCameraImages(imageFolder);
        ProjectorCalibrateTrinsics projectorCalibrateTrinics;
        readTrinsic(trinsicFolder, projectorCalibrateTrinics);

        Images &grayImages = content.cameraGrayImages();
        Mat decodeImage = decode::decodeGrayCodeBrownWoOffset(grayImages, param.imageSize, param.projectorSize);
        Points points;
        Colors colors;
        reconstruct::reconstructBrown(content.cameraImagesWithParam[0]->image, decodeImage, param.imageSize, param.projectorSize, projectorCalibrateTrinics, points, colors);
        std::string filename = savePath + "\\" + "x.ply";
        reconstruct::savePly(filename, points, colors);
    }

    void reconstructeGrayBrownWOffsetImages(std::string imageFolder, std::string trinsicFolder, std::string savePath, InputParamStruct &inputParam)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.grayBrownWOffsetCalibrateParam;
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        content.cameraImagesWithParam = readCameraImages(imageFolder);
        ProjectorCalibrateTrinsics projectorCalibrateTrinics;
        readTrinsic(trinsicFolder, projectorCalibrateTrinics);

        Images &grayImages = content.cameraGrayImages();
        Mat decodeImage = decode::decodeGrayCodeBrownWOffset(grayImages, param.imageSize, param.projectorSize, param.grayCodeColN, param.grayCodeRowN);

        std::string path = "D:\\code\\amiraSl\\test\\decode\\horizontal.tiff";
        Mat horizontal;
        extractChannel(decodeImage, horizontal, 1);
        imwrite(path, horizontal);
        path = "D:\\code\\amiraSl\\test\\decode\\vertical.tiff";
        Mat vertical;
        extractChannel(decodeImage, vertical, 0);
        imwrite(path, vertical);

        Points points;
        Colors colors;
        reconstruct::reconstructBrown(content.cameraImagesWithParam[0]->image, decodeImage, param.imageSize, param.projectorSize, projectorCalibrateTrinics, points, colors);
        std::string saveFolderName = io::getFilenameFromPath(imageFolder);
        std::string saveFolder = io::joinPath(savePath, saveFolderName);
        io::createFolder(saveFolder);
        std::string filename = saveFolder + "\\" + "brownGray.ply";
        reconstruct::savePly(filename, points, colors);
    }

    void monocularCalibrateWithChessboard(std::string folder, InputParamStruct &inputParam, std::string savePath)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.monocularChessboardCalibrateParam;
        content.cameraImagesWithParam = readCameraImages(folder);
        std::vector<Mat> cornerExtractImages;
        int n = (int)content.cameraImagesWithParam.size();
        for (int i = 0; i < n; i++)
        {
            cornerExtractImages.push_back(content.cameraImagesWithParam[i]->image);
        }
        CornersList cornersList = calibrate::extractChessboradCorners(cornerExtractImages, param.cornerCols, param.cornerRows, false, "tradition");
        Points worldCoordinatesCache = calibrate::getWorldCoordinates(param.cornerCols, param.cornerRows, param.squareWidth, param.squareHeight);
        std::vector<Points> worldCoordinates;
        for (int i = 0; i < cornersList.size(); i++)
        {
            worldCoordinates.push_back(worldCoordinatesCache);
        }
        Size imageSize = {content.cameraImagesWithParam[0]->height, content.cameraImagesWithParam[0]->width};
        Intrinsic intrinsic;
        calibrate::cameraCalibrate(worldCoordinates, cornersList, imageSize, intrinsic, 0);
        std::string filename = savePath + "\\" + "intrinsic.txt";
        writeIntrinsic(filename, intrinsic);
    }

    void biocularCalibrateWithChessboard(std::string folder, std::string intrinsicFolder, InputParamStruct &inputParam, std::string savePath)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        auto param = inputParam.biocularChessboardCalibrateParam;
        BiocularCalibrateTrinsics biocularCalibrateTrinsics;
        std::string leftIntrinsicFile = intrinsicFolder + "\\left\\intrinsic.txt";
        readIntrinsic(leftIntrinsicFile, biocularCalibrateTrinsics.leftIntrinsic);
        std::string rightIntrinsicFile = intrinsicFolder + "\\right\\intrinsic.txt";
        readIntrinsic(rightIntrinsicFile, biocularCalibrateTrinsics.rightIntrinsic);
        std::string leftFolder = folder + "\\left";
        std::string rightFolder = folder + "\\right";

        cameraImagesWithParamList.resize(2);
        cameraImagesWithParamList[0] = readCameraImages(leftFolder);
        cameraImagesWithParamList[1] = readCameraImages(rightFolder);
        std::vector<Mat> leftCornerExtractImages, rightCornerExtractImages;
        int n = (int)cameraImagesWithParamList[0].size();
        for (int i = 0; i < n; i++)
        {
            leftCornerExtractImages.push_back(cameraImagesWithParamList[0][i]->image);
            rightCornerExtractImages.push_back(cameraImagesWithParamList[1][i]->image);
        }
        CornersList leftCornersList = calibrate::extractChessboradCorners(leftCornerExtractImages, param.cornerCols, param.cornerRows, false, "tradition");
        CornersList rightCornersList = calibrate::extractChessboradCorners(rightCornerExtractImages, param.cornerCols, param.cornerRows, false, "tradition");

        Points worldCoordinatesCache = calibrate::getWorldCoordinates(param.cornerCols, param.cornerRows, param.squareWidth, param.squareHeight);
        std::vector<Points> worldCoordinates;
        for (int i = 0; i < leftCornersList.size(); i++)
        {
            worldCoordinates.push_back(worldCoordinatesCache);
        }

        calibrate::biocularCameraCalibrate(worldCoordinates, leftCornersList, rightCornersList, param.imageSize, biocularCalibrateTrinsics);
        writeTrinsic(savePath, biocularCalibrateTrinsics);
    }

    void calibrateWithChessboard(std::string monoFolder, std::string stereoFolder, InputParamStruct &inputParam, std::string savePath)
    {
        std::string leftMonoFolder = monoFolder + "\\left";
        monocularCalibrateWithChessboard(leftMonoFolder, inputParam, savePath);
        std::string rightMonoFolder = monoFolder + "\\right";
        monocularCalibrateWithChessboard(rightMonoFolder, inputParam, savePath);
        biocularCalibrateWithChessboard(stereoFolder, savePath, inputParam, savePath);
    }

    void generatePhaseShiftPatternImages(std::string folder, InputParamStruct &inputParam)
    {
        auto param = inputParam.complementaryGrayWithPhaseShiftInputParam;
        Images whiteBlackImages = pattern::generateWhiteBlackImages(param.projectorWidth, param.projectorHeight);
        Images grayCodeImages = pattern::generateGrayCodePatternImages(param.grayCodeN + 1, param.projectorWidth, param.projectorHeight, false, true);
        Images phaseShiftImages = pattern::generatePhaseShiftPatternImages(param);
        Images orderedImages;
        orderedImages.push_back(whiteBlackImages[0]);
        orderedImages.push_back(whiteBlackImages[1]);
        for (int i = 0; i < grayCodeImages.size(); i++)
        {
            orderedImages.push_back(grayCodeImages[i]);
        }
        for (int i = 0; i < phaseShiftImages.size(); i++)
        {
            orderedImages.push_back(phaseShiftImages[i]);
        }
        for (int i = 0; i < orderedImages.size(); i++)
        {
            std::string filename = folder + "\\" + std::to_string(i) + ".png";
            imwrite(filename, orderedImages[i]);
        }
    }

    void reconstructePhaseShiftMonocular(std::string imageFolder, std::string trinsicFolder, std::string savePath, InputParamStruct &inputParam)
    {
        slLog.log("Start reconstruct (CPU)");
        slTimer.start("reconstructePhaseShiftMonocular", "Prepare stage start");
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.complementaryGrayWithPhaseShiftInputParam;
        content.cameraImagesWithParam = readCameraImages(imageFolder);
        ProjectorCalibrateTrinsics projectorCalibrateTrinics;
        readTrinsic(trinsicFolder, projectorCalibrateTrinics);

        Images &grayImagesAll = content.cameraGrayImages();
        Images grayImages, phaseShiftImages;
        for (int i = 0; i < param.grayCodeN + 3; i++)
        {
            grayImages.push_back(grayImagesAll[i]);
        }
        for (int i = param.grayCodeN + 3; i < param.grayCodeN + 3 + param.phaseShiftN; i++)
        {
            phaseShiftImages.push_back(grayImagesAll[i]);
        }

        std::string saveFolderName = io::getFilenameFromPath(imageFolder);
        std::string saveFolder = io::joinPath(savePath, saveFolderName);
        io::createFolder(saveFolder);
        slTimer.record("reconstructePhaseShiftMonocular", "Prepare stage finish, start decode");

        Mat decodeImage = decode::decodeGrayCodeWithPhaseShift(grayImages, phaseShiftImages, param);
        std::string decodeImageFile = io::joinPath(saveFolder, "decodeImage.tiff");
        // Mat x = Mat::zeros(decodeImage.size(), CV_8UC1);
        // for (int h = 0; h < decodeImage.size().height; h++)
        // {
        //     for (int w = 0; w < decodeImage.size().width; w++)
        //     {
        //         x.at<uint8_t>(h, w) = (int)(decodeImage.at<float>(h, w) / 2500 * 255);
        //     }
        // }
        Mat x;
        extractChannel(decodeImage, x, 0);
        imwrite(decodeImageFile, x);
        slTimer.record("reconstructePhaseShiftMonocular", "Decode finish, reconstruct start");
        Points points;
        Colors colors;
        reconstruct::reconstructPhaseShiftMonocular(content.cameraImagesWithParam[0]->image, decodeImage, param.imageSize, param.projectorSize, projectorCalibrateTrinics, param.minDistance, param.maxDistance, points, colors, param.rowGap, param.colGap);
        slTimer.record("reconstructePhaseShiftMonocular", "Reconstruct finish, save start");
        std::string filename = io::joinPath(saveFolder, "phaseShift.ply");
        // reconstruct::savePly(filename, points, colors);
        reconstruct::savePlyBinary(filename, points, colors);
        slLog.log(filename);
        slTimer.record("reconstructePhaseShiftMonocular", "Finish!");
    }

#if USECUDA
    void reconstructePhaseShiftMonocularCuda(std::string imageFolder, std::string trinsicFolder, std::string savePath, InputParamStruct &inputParam)
    {
        slLog.log("Start reconstruct (GPU)");
        slTimer.start("reconstructePhaseShiftMonocularCuda", "Prepare stage start");
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.complementaryGrayWithPhaseShiftInputParam;
        content.cameraImagesWithParam = readCameraImages(imageFolder);
        ProjectorCalibrateTrinsics projectorCalibrateTrinics;
        readTrinsic(trinsicFolder, projectorCalibrateTrinics);

        Images &grayImagesAll = content.cameraGrayImages();
        Images grayImages, phaseShiftImages;
        for (int i = 0; i < param.grayCodeN + 3; i++)
        {
            grayImages.push_back(grayImagesAll[i]);
        }
        for (int i = param.grayCodeN + 3; i < param.grayCodeN + 3 + param.phaseShiftN; i++)
        {
            phaseShiftImages.push_back(grayImagesAll[i]);
        }

        std::string saveFolderName = io::getFilenameFromPath(imageFolder);
        std::string saveFolder = io::joinPath(savePath, saveFolderName);
        io::createFolder(saveFolder);

        slTimer.record("reconstructePhaseShiftMonocularCuda", "Prepare stage finish, start decode");

        Mat decodeImage = decode::decodeGrayCodeWithPhaseShiftCuda(grayImages, phaseShiftImages, param);
        std::string decodeImageFile = io::joinPath(saveFolder, "decodeImage.tiff");
        // Mat x = Mat::zeros(decodeImage.size(), CV_8UC1);
        // for (int h = 0; h < decodeImage.size().height; h++)
        // {
        //     for (int w = 0; w < decodeImage.size().width; w++)
        //     {
        //         x.at<uint8_t>(h, w) = (int)(decodeImage.at<float>(h, w) / 2500 * 255);
        //     }
        // }
        Mat x;
        extractChannel(decodeImage, x, 0);
        imwrite(decodeImageFile, x);
        slTimer.record("reconstructePhaseShiftMonocularCuda", "Decode finish, reconstruct start");
        Points points;
        Colors colors;
        reconstruct::reconstructPhaseShiftMonocularCuda(content.cameraImagesWithParam[0]->image, decodeImage, param.imageSize, param.projectorSize, projectorCalibrateTrinics, param.minDistance, param.maxDistance, points, colors);
        slTimer.record("reconstructePhaseShiftMonocularCuda", "Reconstruct finish, save start");

        std::string filename = io::joinPath(saveFolder, "phaseShift.ply");
        // reconstruct::savePly(filename, points, colors);
        reconstruct::savePlyBinary(filename, points, colors);
        slTimer.record("reconstructePhaseShiftMonocularCuda", "Finish!");
    }
#endif

    void reconstructePhaseShiftBiocular(std::string imageFolder, std::string trinsicFolder, std::string savePath, InputParamStruct &inputParam)
    {
        SlContent &content = SL::SLContent::getInstance().getContent();
        auto param = inputParam.complementaryGrayWithPhaseShiftBiocularInputParam;
        auto &cameraImagesWithParamList = content.cameraImagesWithParamList;
        BiocularCalibrateTrinsics biocularCalibrateTrinsics;
        readTrinsic(trinsicFolder, biocularCalibrateTrinsics);
        cameraImagesWithParamList.resize(2);
        Mat decodeImages[2];
        for (int i = 0; i < 2; i++)
        {
            std::string folderName = i == 0 ? "left" : "right";
            std::string leftImageFolder = imageFolder + "\\" + folderName;
            cameraImagesWithParamList[i] = readCameraImages(leftImageFolder);
            Images &grayImagesAll = content.cameraGrayImages(i);
            Images grayImages, phaseShiftImages;
            for (int i = 0; i < 7; i++)
            {
                grayImages.push_back(grayImagesAll[i]);
            }
            for (int i = 7; i < 7 + param.phaseShiftN; i++)
            {
                phaseShiftImages.push_back(grayImagesAll[i]);
            }
            decodeImages[i] = decode::decodeGrayCodeWithPhaseShift(grayImages, phaseShiftImages, param.complementaryGrayWithPhaseShiftInputParam);
        }
        Mat rectifiedDecodeImageL, rectifiedDecodeImageR, projectionMatrix;
        rectify(decodeImages[0], decodeImages[1], biocularCalibrateTrinsics.leftIntrinsic, biocularCalibrateTrinsics.rightIntrinsic, biocularCalibrateTrinsics.extrinsic, rectifiedDecodeImageL, rectifiedDecodeImageR, projectionMatrix);

        Mat grayImageL, grayImageR, projectionMatrix2;
        rectify(content.cameraGrayImages(0)[0], content.cameraGrayImages(0)[1], biocularCalibrateTrinsics.leftIntrinsic, biocularCalibrateTrinsics.rightIntrinsic, biocularCalibrateTrinsics.extrinsic, grayImageL, grayImageR, projectionMatrix2);

        Mat depth, disparity;
        Points points, colors;
        reconstruct::reconstructPhaseShiftBiocular(rectifiedDecodeImageL, rectifiedDecodeImageR, grayImageL, biocularCalibrateTrinsics, projectionMatrix, points, colors, disparity, depth);
        std::string depthFile = savePath + "\\depth.tiff";
        imwrite(depthFile, depth);
        std::string disparityFile = savePath + "\\disparity.tiff";
        imwrite(disparityFile, disparity);

        std::string decodeLImagePath = savePath + "\\decodeImageL.tiff";
        std::string decodeRImagePath = savePath + "\\decodeImageR.tiff";
        imwrite(decodeLImagePath, decodeImages[0]);
        imwrite(decodeRImagePath, decodeImages[1]);

        std::string rectifiedLImagePath = savePath + "/rectifiedImageL.tiff";
        std::string rectifiedRImagePath = savePath + "/rectifiedImageR.tiff";
        imwrite(rectifiedLImagePath, rectifiedDecodeImageL);
        imwrite(rectifiedRImagePath, rectifiedDecodeImageR);

        std::string filename = savePath + "\\" + "phaseShiftBiocular.ply";
        reconstruct::savePly(filename, points, colors);
    }

    void testRectifyImage(Mat imageL, Mat imageR, std::string trinsicFolder, std::string savePath)
    {
        BiocularCalibrateTrinsics biocularCalibrateTrinsics;
        readTrinsic(trinsicFolder, biocularCalibrateTrinsics);

        Mat rectifiedImageL, rectifiedImageR, projectionMatrix;
        rectify(imageL, imageR, biocularCalibrateTrinsics.leftIntrinsic, biocularCalibrateTrinsics.rightIntrinsic, biocularCalibrateTrinsics.extrinsic, rectifiedImageL, rectifiedImageR, projectionMatrix);

        std::string lImagePath = savePath + "/imageL.png";
        std::string rImagePath = savePath + "/imageR.png";
        std::string rectifiedLImagePath = savePath + "/rectifiedImageL.png";
        std::string rectifiedRImagePath = savePath + "/rectifiedImageR.png";
        imwrite(lImagePath, imageL);
        imwrite(rImagePath, imageR);
        imwrite(rectifiedLImagePath, rectifiedImageL);
        imwrite(rectifiedRImagePath, rectifiedImageR);
    }
}